/*************************************************************************
*
*	platformstatus.c
*
*  Copyright 2007,2008,2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for storing and retrieving the 
*    current STATUS and COMMAND structs.
*
**************************************************************************
*
*  version 1.0.4, 2/23/09, SBS
*    - add platformtype in STATUS
*    - cleanup
*
*  version 1.0.3, 2/2/09, SBS
*    - rename global vars to global_*
*
*  version 1.0.2, 2/1/09, SBS
*    - reconcile with new COMMAND struct
*    - reconcile with new STATUS struct
*
*  version 1.0.1, 1/30/09, SBS
*    - replace CONFIGDATA as parameter to _Init and _Update
*    - add _SetMode
*    - move controller_InitDefault() into _init
*    - have _GetStatus and _GetCommand call _Init instead of 
*        status_Init if uninitialized
*
*  version 1.0, 1/9/09, SBS
*    - first version, derived from platform.c
*
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup platform
*  @brief
*  This module provides functions for retrieving the current STATUS
*    and COMMAND structs.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <time.h>
#include <pthread.h>

#include "types.h"

#include "waypoints.h"
#include "platform_types.h"
#define NEED_MODENAMES
#include "platform_modes.h"
#undef NEED_MODENAMES
#include "status.h"

#include "command.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "engineering_main.h"
#include "science_main.h"

#include "controller.h"
#include "platformstatus.h"


//*****************************************************************************
static STATUS global_status;
static COMMAND global_cmd;
static int global_commandchanged;
static BYTE global_initialized=0;

static pthread_mutex_t status_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t command_mutex = PTHREAD_MUTEX_INITIALIZER;

static pthread_mutex_t flag_mutex = PTHREAD_MUTEX_INITIALIZER;

/**
***************************************************************************
*
* platformstatus_Init()
*/
/** 
*
* @brief
* Initialize STATUS and COMMAND structs.
*
*
**************************************************************************/
void platformstatus_Init(
			int boatnum
			,char *boatname
			,int plattype)
{		
	STATUS tempstatus;
	COMMAND tempcmd;

	printf("platformstatus_Init()\n");
	
	tempstatus = engineering_InitStatus(boatnum,boatname,plattype);

	printf("platformstatus_Init()\n");

	pthread_mutex_lock( &status_mutex );
	global_status = tempstatus;
	pthread_mutex_unlock( &status_mutex );

	controller_InitDefault();

	tempcmd = command_InitCommand();
	tempcmd.nCmdID = CMD_HALT;
	tempcmd.nWaypointCount = 0;

	pthread_mutex_lock( &command_mutex );
	global_cmd = tempcmd;
	pthread_mutex_unlock( &command_mutex );
	
	pthread_mutex_lock( &flag_mutex );
	global_commandchanged = 1;	
	global_initialized = 1;
	pthread_mutex_unlock( &flag_mutex );
	
	printf("platformstatus_Init()\n");
	
	return;
}


/**
***************************************************************************
*
* platformstatus_GetStatus()
*/
/** 
*
* @brief
* Returns the current STATUS struct.
*
*
**************************************************************************/
STATUS platformstatus_GetStatus(void)
{
	STATUS rval;
	
	pthread_mutex_lock( &flag_mutex );
	if(global_initialized==0){
		printf("platformstatus_GetStatus: Not initialized!\n");
		pthread_mutex_unlock( &flag_mutex );

		platformstatus_Init(1,"NO NAME",0);

	}else{
		pthread_mutex_unlock( &flag_mutex );
	}
	
	pthread_mutex_lock( &status_mutex );
	rval = global_status;
	pthread_mutex_unlock( &status_mutex );

	return(rval);
}


/**
***************************************************************************
*
* platformstatus_GetCommand()
*/
/** 
*
* @brief
* Returns the current COMMAND struct
*
*
**************************************************************************/
COMMAND platformstatus_GetCommand(void)
{
	COMMAND rval;

	pthread_mutex_lock( &flag_mutex );
	if(global_initialized==0){
		printf("platformstatus_GetCommand: Not initialized!\n");
		pthread_mutex_unlock( &flag_mutex );

		platformstatus_Init(1,"NO NAME",0);

	}else{
		pthread_mutex_unlock( &flag_mutex );
	}	
	pthread_mutex_lock( &command_mutex );
	rval = global_cmd;
	pthread_mutex_unlock( &command_mutex );

	return(rval);
}


/**
***************************************************************************
*
* platformstatus_SetCommand()
*/
/** 
*
* @brief
*  Replaces the current COMMAND struct
*
*
**************************************************************************/
void platformstatus_SetCommand(COMMAND c)
{
	pthread_mutex_lock( &command_mutex );
	global_cmd = c;
	pthread_mutex_unlock( &command_mutex );

	pthread_mutex_lock( &flag_mutex );
	global_commandchanged = 1;
	pthread_mutex_unlock( &flag_mutex );

	return;
}


/**
***************************************************************************
*
* platformstatus_SetCommand()
*/
/** 
*
* @brief
*  Replaces the current COMMAND struct
*
*
**************************************************************************/
void platformstatus_SetMode(int mode)
{
	pthread_mutex_lock( &status_mutex );
	global_status.nPlatformMode = mode;
	
	snprintf(global_status.sPlatformModeName, sizeof(global_status.sPlatformModeName)
			, "%s",  modenames[global_status.nPlatformMode]);

	printf("Setting mode to %s\n",global_status.sPlatformModeName);
	pthread_mutex_unlock( &status_mutex );
	
	return;
}


/**
***************************************************************************
*
* platformstatus_CommandChanged()
*/
/** 
*
* @brief
*  Has command changed since last time we asked?
*
*
**************************************************************************/
int platformstatus_CommandChanged()
{
	pthread_mutex_lock( &flag_mutex );
	if(global_commandchanged == 1){
		global_commandchanged = 0;
		pthread_mutex_unlock( &flag_mutex );
		return(1);
	}else{
		pthread_mutex_unlock( &flag_mutex );
		return(0);
	}
}


/**
***************************************************************************
*
* platformstatus_UpdateStatus()
*/
/** 
*
* @brief
*  Calls engineering and science update routines to
*    update STATUS  
*
*  Parameters:
*    - cf - configuration data
*
*  Returns:
*    - none
*
**************************************************************************/
void platformstatus_UpdateStatus()
//			CONFIGDATA cf)
{

	POSITION p;
	STATUS tempstatus;

	pthread_mutex_lock( &flag_mutex );
	if(global_initialized==0){
		pthread_mutex_unlock( &flag_mutex );
		printf("platformstatus_UpdateStatus: Not initialized!\n");

		platformstatus_Init(1,"NO NAME",0);

	}else{
		pthread_mutex_unlock( &flag_mutex );
	}
		
	#ifdef DEBUG
	logfile_LogLevelPrint(LOGLEVEL_DEBUG,"update_status()");
	#endif		
	

	tempstatus = engineering_update();

	p = status_PositionFromStatus(tempstatus);
	
	#ifdef SCIENCE_SIMULATE_AIRPRESSURE		
	tempstatus.dAirPress_mbar   = science_main(AIRPRESSURE,p);
	#endif

	#if defined(SCIENCE_SIMULATE_AIRTEMP) || defined(SCIENCE_USE_GOTEMP_AIRTEMP)
	tempstatus.dAirTempDegC     = science_main(AIRTEMP,p);
	#endif

	#ifdef SCIENCE_SIMULATE_BATHYMETRY		
	tempstatus.dBathymetry      = science_main(BATHYMETRY,p);
	#endif

	#ifdef SCIENCE_SIMULATE_FLUOROMETER		
	tempstatus.dFluorescence_V  = science_main(FLUORESCENCE,p);
	#endif

	#ifdef SCIENCE_SIMULATE_HUMIDITY		
	tempstatus.dHumidPct        = science_main(HUMIDITY,p);
	#endif

	#ifdef SCIENCE_SIMULATE_SALINITY		
	tempstatus.dSalinity_psu    = science_main(SALINITY,p);
	#endif

	#ifdef SCIENCE_SIMULATE_CURRENT		
	tempstatus.dCurrentSpeedMPS = science_main(CURRENTSPEED,p);
	tempstatus.dCurrentAngleDeg = science_main(CURRENTANGLE,p);
	#endif

	#ifdef SCIENCE_SIMULATE_WATERTEMP		
	tempstatus.dWaterTempDegC   = science_main(WATERTEMP,p);
	#endif

	#ifdef SCIENCE_SIMULATE_WIND		
	tempstatus.dWindSpeed_mps   = science_main(WINDSPEED,p);
	tempstatus.dWindAngleDeg    = science_main(WINDANGLE,p);
	#endif

	pthread_mutex_lock( &status_mutex );
	global_status = tempstatus;	
	pthread_mutex_unlock( &status_mutex );

	return;
}

